#include "motor.h"
#include "pwm.h"
#include "abz_encoder.h"
#include "hall_sensor.h"
#include "led_control.h"
#include <string.h>
// 电机状态实例
motor_state_t motor_state = {0};
// 电机配置实例
motor_config_t motor_config = {0};
// 电机控制目标结构体实例
motor_target_t motor_target = {0};

// 电机参数初始化
void motor_parameter_init(void)
{
  motor_target.ctrl_mode = MOTOR_CTRL_OPEN_LOOP;    // 开环控制
  motor_target.work_mode = MOTOR_MODE_POSITION;     // 位置模式

  motor_config.cf_kp = 2.0f;
  motor_config.cf_ki = 0.01f;
  motor_config.cf_kd = 0;
}


void Set_Final_Vd(void)
{
  if (motor_target.work_mode == MOTOR_MODE_STOP)
  {
    motor_target.voltage_d = 0.0f;
  } else if (motor_target.work_mode == MOTOR_MODE_INIT) // 初始化
  {
    if (motor_target.ctrl_mode == MOTOR_CTRL_CLOSED_LOOP)   // 闭环控制
    {
      motor_target.voltage_d = motor_target.loop_output.voltage_d;
    } else {    // 开环控制
      motor_target.voltage_d = motor_target.loop_output.torque;
    }
  } else if (motor_target.work_mode == MOTOR_MODE_POSITION) // 位置模式
  {
    if (motor_target.ctrl_mode == MOTOR_CTRL_CLOSED_LOOP)   // 闭环控制
    {
      motor_target.voltage_d = motor_target.loop_output.voltage_d;
    } else {    // 开环控制
      motor_target.voltage_d = motor_target.loop_output.torque;
    }
  } else if (motor_target.ctrl_mode == MOTOR_MODE_TORQUE)   // 力矩模式
  {
    if (motor_target.ctrl_mode == MOTOR_CTRL_CLOSED_LOOP)   // 闭环控制
    {
      motor_target.voltage_d = motor_target.loop_output.voltage_d;
    } else
    {
      motor_target.voltage_d = motor_target.user_target.voltage_d;
    }
  } else
  {
    motor_target.voltage_d = 0.0f;
  }
  
}
